// Copyright 2020 Tier IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
//
// Author: v1.0 Yukihiro Saito
//

#ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_
#define MULTI_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_

#define EIGEN_MPL2_ONLY
#include "multi_object_tracker/utils/utils.hpp"
// #include "object_recognition_utils/object_recognition_utils.hpp"
#include "common/algorithm_type/lidar_cell_objs_type.h"
#include <Eigen/Core>
// #include <rclcpp/rclcpp.hpp>

// #include "autoware_auto_perception_msgs/msg/detected_object.hpp"
// #include "autoware_auto_perception_msgs/msg/tracked_object.hpp"
// #include "geometry_msgs/msg/point.hpp"
// #include "unique_identifier_msgs/msg/uuid.hpp"

#include <vector>
namespace perception
{
    namespace algorithm
    {
        class Tracker
        {
        protected:
            // TypeAndScore classification_;
            uint32_t getUUID() const { return uuid_; }
            void setClassification(
                const std::vector<TypeAndScore> &classification)
            {
                classification_ = classification;
            }
            void updateClassification(
                const std::vector<TypeAndScore> &classification);

        private:
            uint32_t uuid_;
            std::vector<TypeAndScore> classification_; // 这里其实不对,这里是拿得一帧数据的所有可能类别及可能性,但我改成了所有跟踪帧数的类别及可能性
            int no_measurement_count_;
            int total_no_measurement_count_;
            int total_measurement_count_;
            double last_update_with_measurement_time_;

        public:
            Tracker(
                const double &time, const LidarObject &obj);
            virtual ~Tracker() {}
            bool updateWithMeasurement(
                const LidarObject &object,
                const double &measurement_time);
            bool updateWithoutMeasurement();
            std::vector<TypeAndScore> getClassification() const
            {
                return classification_;
            }
            inline std::uint8_t getHighestProbLabel(
                const std::vector<TypeAndScore> &object_classifications)
            {
                auto classification = getHighestProbClassification(object_classifications);
                return classification.type;
            }
            int getNoMeasurementCount() const { return no_measurement_count_; }
            int getTotalNoMeasurementCount() const { return total_no_measurement_count_; }
            int getTotalMeasurementCount() const { return total_measurement_count_; }
            double getElapsedTimeFromLastUpdate(const double &current_time) const
            {
                return (current_time - last_update_with_measurement_time_);
            }
            virtual PoseAndCov getPoseWithCovariance( // 前三个是xyz,后36是协方差
                const double &time) const;

            /*
             * Pure virtual function
             */

        protected:
            virtual bool measure(
                const LidarObject &object,
                const double &measurement_time) = 0;

        public:
            virtual bool getTrackedObject(
                const double &time,
                LidarObject &object) const = 0;
            virtual bool predict(const double &time) = 0;
        };
    }
}
#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_
